package uk.ac.cam.djjb2.mygame;

import java.util.AbstractMap;
import java.util.ArrayDeque;
import java.util.Comparator;
import java.util.Deque;
import java.util.Set;
import java.util.concurrent.RunnableFuture;

public class ThetaStarPathFinder implements PathFinder {
	private static class Cell {
		Integer openSetIndex;
		boolean closed;
		double F, G;
		Vector2i prev;
	}
	
	private AttributeMap map;
	private PriorityQueueSet<Vector2i> openSet;
	private Cell[][] cellArray;
	
	public ThetaStarPathFinder(AttributeMap map) {
		this.map = map;
		this.cellArray = new Cell[map.getSize().y][map.getSize().x];
		this.openSet = new PriorityQueueSet<Vector2i>(new AbstractMap<Vector2i, Integer>() {

			@Override
			public Integer get(Object paramObject) {
				Vector2i a = (Vector2i)paramObject;
				Cell cell = cellArray[a.y][a.x];
				if (cell == null) {
					cellArray[a.y][a.x] = cell = new Cell();
				}
				return cell.openSetIndex;
			}
			
			@Override
			public Integer put(Vector2i a, Integer i) {
				Cell cell = cellArray[a.y][a.x];
				if (cell == null) {
					cellArray[a.y][a.x] = cell = new Cell();
				}
					
				Integer result = cell.openSetIndex;
				cell.openSetIndex = i;
				
				return result;
			}
			
			@Override
			public Integer remove(Object paramObject) {
				Vector2i a = (Vector2i)paramObject;
				Integer result = cellArray[a.y][a.x].openSetIndex;
				cellArray[a.y][a.x].openSetIndex = null;
				return result;
			}
			
			@Override
			public boolean containsKey(Object paramObject) {
				return get(paramObject) != null;
			}
			
			@Override
			public Set<java.util.Map.Entry<Vector2i, Integer>> entrySet() {
				throw new RuntimeException();
			}
		}, new Comparator<Vector2i>() {

			@Override
			public int compare(Vector2i a, Vector2i b) {
				return Double.compare(cellArray[a.y][a.x].F, cellArray[b.y][b.x].F);
			}
		});
	}
	
	Cell getCell(Vector2i a) {
		Cell cell = cellArray[a.y][a.x];
		if (cell == null) {
			cellArray[a.y][a.x] = cell = new Cell();
		}
		return cell;
	}
	
	Path getPath(final Vector2i to) {
		return new Path() {
			private Deque<Vector2i> nodes = new ArrayDeque<Vector2i>();
			private Vector2i current = null;
			private boolean isInvalid = false;
			
			{
				Vector2i pos = to;
				while (pos != null) {
					nodes.addLast(pos);
					pos = getCell(pos).prev;
				}
				current = nodes.getLast();
			}
			
			@Override
			public void progress(Vector2i pos) {
				if (!current.equals(pos)) {
					if (nodes.getLast().equals(pos)) {
						current = nodes.removeLast();
					}
				}
			}
			
			@Override
			public boolean isInvalid() {
				return isInvalid;
			}
			
			@Override
			public boolean isEmpty() {
				return nodes.isEmpty();
			}
			
			@Override
			public Vector2i getCurrentTarget() {
				return nodes.getLast();
			}
			
			@Override
			public void cancel() {
				isInvalid = true;
				nodes.clear();
			}

			@Override
			public Vector2i getDestination() {
				return nodes.getFirst();
			}
		};
	}
	
	boolean raycast(Vector2i a, Vector2i b) {
		int dx=a.x-b.x;
		int dy=b.y-a.y;
		
		int ddx = dx > 0 ? -1 : 1;
		int ddy = dy < 0 ? -1 : 1;
		
		dx = -Math.abs(dx);
		dy = Math.abs(dy);

		int D = dx+dy;
		dx *= 2;
		dy *= 2;

		int x = a.x;
		int y = a.y;

		while(x != b.x || y != b.y) {
			if (D > 0) {
				y += ddy;
				D += dx;
			} else {
				x += ddx;
				D += dy;
			}
			if (map.get(new Vector2i(x, y)))
				return false;
		}
		
		return true;
	}
	
	@Override
	public RunnableFuture<Path> findPath(final Vector2i from, final Vector2i to) {
		return new CancellableTask<Path>() {
			@Override
			protected Path execute() throws Throwable {

				openSet.add(from);
				while (!isCancelling() && !openSet.isEmpty()) {
					Vector2i pos = openSet.remove();
					Cell cell = getCell(pos);
					cell.closed = true;
					
					if (pos.equals(to)) {
						Path result = getPath(to);
						return result;
					}
					
					Vector2i[] adjs = new Vector2i[] {
							pos.add(-1, 0),
							pos.add(1, 0),
							pos.add(0, -1),
							pos.add(0, 1)
					};
					
					for (Vector2i adj : adjs) {
						if (adj.x >= 0 && adj.y >= 0 && adj.x < map.getSize().x && adj.y < map.getSize().y && !map.get(adj)) {
							Cell c = getCell(adj);
							if (!c.closed) {
								Vector2i prev = pos;
								Cell p;
								
								while ((p = getCell(prev)).prev != null && raycast(adj, p.prev)) {
									prev = p.prev;
								}
								
								double G = p.G + prev.sub(adj).length();
								if (c.openSetIndex != null) {
									if (c.G > G) {
										c.F += G - c.G;
										c.G = G;
										c.prev = prev;
										openSet.update(adj);
									}
								} else {
									c.G = G;
									c.F = G + adj.sub(to).length();
									c.prev = prev;
									openSet.add(adj);
								}
							}
						}
					}
				}
				
				return null;
			}
		};
	}

}
